
import java.awt.BasicStroke;
import java.awt.Color;
import java.awt.Container;
import java.awt.FlowLayout;
import java.awt.Graphics;
import java.awt.Graphics2D;
import java.awt.event.ActionEvent;
import java.awt.event.ActionListener;
import java.awt.geom.Line2D;
import java.io.DataInputStream;
import java.io.DataOutputStream;

import javax.swing.JButton;
import javax.swing.JFrame;

import lejos.pc.comm.NXTConnector;

public class LocalizationMonitor extends JFrame implements ActionListener
{
	
	static final int FORWARD = 1;
	static final int BACKWARD = -1;
	static final int STOP = 0;
	
	static int command = STOP;
	
	static Map map;
	static MCL mcl;
	
	static DataOutputStream dos;
	static DataInputStream dis;
	static NXTConnector conn;
	
	JButton forward_btn;
	JButton backward_btn;
	JButton stop_btn;

	LocalizationMonitor()
	{
		
		super("Localization Monitor for CmpE 49A Fall 2009 - Lab #9");
		setSize( 700, 500 );
		
		Container content = this.getContentPane();
		this.setLayout(new FlowLayout());
		this.setBackground(Color.white);
		
		forward_btn =  new JButton("Forward");
		backward_btn =  new JButton("Backward");
		stop_btn =  new JButton("Stop");
		
		forward_btn.addActionListener(this);
		backward_btn.addActionListener(this);
		stop_btn.addActionListener(this);
		
		forward_btn.setActionCommand("forward");
		backward_btn.setActionCommand("backward");
		stop_btn.setActionCommand("stop");
		
		content.add(forward_btn);
		content.add(backward_btn);
		content.add(stop_btn);
		
		setVisible( true );
		
		map = new Map();
		
		// Replace this with your selection of number of particles
		mcl = new MCL( 300 , map );
		
		conn = new NXTConnector();
		
	}
	
	public static void main(String[] args) throws Exception
	{
		
		int motion;
		int distance;
		String bluetoothAddr = "0016530401ED";
		
		LocalizationMonitor monitor = new LocalizationMonitor();
		
		monitor.setDefaultCloseOperation( JFrame.EXIT_ON_CLOSE );
		
		if (args.length == 1) {
			bluetoothAddr = args[0];
		}
		
		// Connect to any NXT over Bluetooth
		boolean connected = conn.connectTo("btspp://" );
		//boolean connected = conn.connectTo("btspp://" + bluetoothAddr + ":1" );
		
		if (!connected) {
			System.err.println("Failed to connect to any NXT");
			System.exit(1);
		}
		
		//is = conn.getInputStream();
		//str = new DataInputStream( is );
		
		dis = conn.getDataIn();
		dos = conn.getDataOut();
		
		while( true )
		{
			dos.writeInt(command);
			dos.flush();
			
			motion = dis.readInt();
			distance = dis.readInt();
			//input the odometry information
			mcl.motionUpdate(motion);
			//input the ultrasonic sensor information
			mcl.measurementUpdate(distance);
			//mcl.calculatePose();
			System.out.println(motion + " " + distance);
			monitor.repaint();
		}
		
		/*
		map = new Map();
		mcl = new MCL(100, map);
		mcl.calculateDistribution(, mean)
		*/
	}

	
	public void paint( Graphics g )
	{
		super.paint( g );
		displayMap( map, g); 
		displayParticles( mcl.particles, g );
		displayPose( mcl.pose, g );
	}

	public void displayMap( Map map, Graphics g )
	{
		Graphics2D g2 = ( Graphics2D ) g;
		g2.setPaint( Color.blue );
		g2.setStroke( new BasicStroke( 5.0f ));

		for ( int i = 1; i < MCL.WORLD_SIZE; i++ )
		{
			g2.draw( new Line2D.Double( ( int ) ( ( i - 1 )*3 ) , 180 - map.expectedDistance( i - 1 )  , ( int ) ( i * 3 ), 180 - map.expectedDistance( i ) ) );
		}
		
	}
	
	public void displayParticles( Particle particles[], Graphics g )
	{
		for ( int i = 0; i < mcl.NUM_PARTICLES ; i++ )
		{
			displayParticle( particles[ i ], g );
		}
	}
	public void displayParticle( Particle particle, Graphics g )
	{
		Graphics2D g2 = ( Graphics2D ) g;
		g2.setPaint( Color.red );
		g2.setStroke( new BasicStroke( 2.0f ));
		
		g2.draw( new Line2D.Double( ( int ) ( particle.location * 3 ), 300, ( int )( particle.location * 3 ), ( int )( 300 - 100*particle.belief ) ) );
	}
	
	public void displayPose( Particle particle, Graphics g )
	{
		Graphics2D g2 = ( Graphics2D ) g;
		g2.setPaint( Color.blue );
		g2.setStroke( new BasicStroke( 5.0f ));
		
		g2.draw( new Line2D.Double( ( int ) ( particle.location * 3 ), 300, ( int )( particle.location * 3 ), ( int )( 300 - 100*particle.belief ) ) );
				
	}

	@Override
	public void actionPerformed(ActionEvent e) {
		if ("forward".equals(e.getActionCommand())) {
			System.out.println("forward is pressed");
			command = FORWARD;
		} else if ("backward".equals(e.getActionCommand())){
			System.out.println("backward is pressed");
			command = BACKWARD;
		} else if ("stop".equals(e.getActionCommand())) {
			System.out.println("stop is pressed");
			command = STOP;
			
		}
	}

}
